from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription, ExecuteProcess
from launch.launch_description_sources import PythonLaunchDescriptionSource
from ament_index_python.packages import get_package_share_directory
import os

def generate_launch_description():
    ld = LaunchDescription()

    # 包含 fishbot_description 的 gazebo_sim 启动文件
    gazebo_sim_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(
                get_package_share_directory('fishbot_description'),
                'launch',
                'gazebo_sim.launch.py')))
    ld.add_action(gazebo_sim_launch)

    # 包含 fishbot_navigation2 的 navigation 启动文件
    navigation_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(
                get_package_share_directory('fishbot_navigation2'),
                'launch',
                'navigation2.launch.py')))
    ld.add_action(navigation_launch)

    # 运行 fishbot_application 的 init_robot_pose 节点
    init_robot_pose_node = ExecuteProcess(
        cmd=['ros2', 'run', 'fishbot_application', 'init_robot_pose'],
        output='screen')
    ld.add_action(init_robot_pose_node)

    # 运行 fishbot_application 的 navi_to_pose 节点
    navi_to_pose_node = ExecuteProcess(
        cmd=['ros2', 'run', 'fishbot_application', 'navi_to_pose'],
        output='screen')
    ld.add_action(navi_to_pose_node)

    return ld